package com.dji.mobilesdk.vision;


import android.util.Log;
import java.util.ArrayList;
import java.util.List;
import org.opencv.aruco.Aruco;
import org.opencv.aruco.Dictionary;
import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfInt;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;


public class OpenCVHelper {

    public OpenCVHelper() {
    }

    //aruco二维码检测
    public Mat detectArucoTag(Mat input, Dictionary dictionary, DroneHelper droneHelper) {
        Mat grayImgMat = new Mat();
        Mat intermediateImgMat = new Mat();
        Mat output = new Mat();
        Imgproc.cvtColor(input, grayImgMat, Imgproc.COLOR_RGBA2GRAY);
        Imgproc.cvtColor(input, intermediateImgMat, Imgproc.COLOR_RGBA2RGB);
        MatOfInt ids = new MatOfInt();
        List<Mat> corners = new ArrayList<>();

        Aruco.detectMarkers(grayImgMat, dictionary, corners, ids);
        if (ids.depth() > 0) {
            Aruco.drawDetectedMarkers(intermediateImgMat, corners, ids, new Scalar(255, 0, 255));

            //旋转矩阵
            Mat rvecs = new Mat();
            //位移矩阵
            Mat tvecs = new Mat();
            //相机矩阵
            Mat cameraMatrix = new Mat();
            //畸变
            Mat distCoeffs = new Mat();
            cameraMatrix = Mat.eye(3, 3, CvType.CV_64FC1);
            distCoeffs = Mat.zeros(5, 1, CvType.CV_64FC1);

            Aruco.estimatePoseSingleMarkers(corners, 0.15f, cameraMatrix, distCoeffs, rvecs, tvecs);

            for(int i=0; i<ids.toArray().length; i++){
                Aruco.drawAxis(intermediateImgMat, cameraMatrix, distCoeffs, rvecs.row(i), tvecs.row(i), 0.02f);
            }

            moveOnArucoDetected(ids, corners, droneHelper, intermediateImgMat.width(), intermediateImgMat.height());
        }
        Imgproc.cvtColor(intermediateImgMat, output, Imgproc.COLOR_RGB2BGR);
        Imgproc.cvtColor(output, output, Imgproc.COLOR_BGR2RGBA, 4);
        grayImgMat.release();
        intermediateImgMat.release();
        return output;
    }

    //飞机移动
    private void moveOnArucoDetected(Mat ids, List<Mat> corners, DroneHelper droneHelper, int imageWidth, int imageHeight) {
        Scalar markerCenter = new Scalar(0, 0);
        for (Mat corner : corners) {
            markerCenter = Core.mean(corner);
        }

        Scalar imageVector = new Scalar(markerCenter.val[0] - imageWidth / 2f, markerCenter.val[0] - imageHeight / 2f);

        Scalar motionVector = convertImageVectorToMotionVector(imageVector);

        if (ids.size().empty()) {
            motionVector = new Scalar(0, 0);
        }

        Log.d("OpenCVHelper", "Moving By: " + motionVector.toString());
        if ((imageVector.val[0] * imageVector.val[0] + imageVector.val[1] * imageVector.val[1]) < 900) {
            droneHelper.moveVxVyYawrateVz((float) motionVector.val[0], (float) motionVector.val[1], 0f, -0.2f);
        } else {
            droneHelper.moveVxVyYawrateVz((float) motionVector.val[0], (float) motionVector.val[1], 0f, 0f);
        }
    }

    //将图像矢量转换为飞机移动矢量
    private Scalar convertImageVectorToMotionVector(Scalar imageVector) {
        double pX = -imageVector.val[1];
        double pY = imageVector.val[0];
        double divisor = Math.sqrt((pX * pX) + (pY * pY));
        pX = pX / divisor;
        pY = pY / divisor;
        return new Scalar(pX * 0.2, pY * 0.2);
    }
}
